package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.SimpleRobot;
import java.io.IOException;
import java.util.Timer;
import java.util.TimerTask;
import org.nashua.tt151.libraries.Controller.DualAction;
import org.nashua.tt151.systems.Camera;
import org.nashua.tt151.systems.DriveTrain;
import org.nashua.tt151.systems.Shooter;

public class Alpha extends SimpleRobot {
	private final DualAction driver = new DualAction( 1, 0.1 );
	private final DualAction shooter = new DualAction( 2, 0.1 );

	private Dash dash;
	private boolean tryingToConnect = false;
	
	private void connectToDashboard() {
		if ( tryingToConnect ) {
			return;
		}
		tryingToConnect = true;
		new Thread() {
			public void run() {
				try {
					if ( tryingToConnect && dash == null ) {
						dash = new Dash();
					}
				} catch ( IOException ex ) {
					System.err.println( "[ERR Failed to connect to dashboard: " + ex.getMessage() + "]" );
					dash = null;
				}
				tryingToConnect = false;
			}
		}.start();
	}

	public void robotInit() {
		connectToDashboard();
		new Timer().scheduleAtFixedRate( new TimerTask() {
			int failedCount = 0;

			public void run() {
				if ( dash != null ) {
					try {
						dash.testConnection();
						failedCount = 0;
					} catch ( IOException ex ) {
						failedCount++;
						if ( failedCount <= 10 ) {
							failedCount = 0;
							dash = null;
							connectToDashboard();
						}
						System.err.println( "DISCONNECTED FROM DASH" );
					}
				} else {
					failedCount = 0;
					connectToDashboard();
				}
			}
		}, 1, 100 );
	}

	public void autonomous() {
		try {
			DriveTrain.getInstance().set( -0.5, 0.5);
			Thread.sleep( 2000 );
			DriveTrain.getInstance().set( 0.0, 0.0);
		} catch ( InterruptedException ex ) {
			ex.printStackTrace();
		}
	}

	public void operatorControl() {
		DriveTrain.getInstance().enabled = true;
		// Initialize all subsystems
		Camera.getInstance().init();
		DriveTrain.getInstance().init();
		Shooter.getInstance().init();
		// Operator control loop
		while ( isEnabled() && isOperatorControl() ) {
			driver.queryButtons();
			shooter.queryButtons();
			
			Camera.getInstance().operatorControl( driver, shooter );
			DriveTrain.getInstance().operatorControl( driver, shooter );
			Shooter.getInstance().operatorControl( driver, shooter );
		}
		DriveTrain.getInstance().enabled = false;
	}

	public void test() {}
}
